#include <INIReader.h>
#include <flashai/commons/file_utils.h>
#include <flashai/commons/spdlog_utils.h>
#include <ros/ros.h>

#include "custom_msg/RANGE.h"
#include "flashros/commons/utils.h"
#include "flashros_commons/BoxesMsg.h"

class LaserHelper {
 public:
  LaserHelper() {}
  void Init(int argc, char** argv) {
    // const params
    std::string input_section = "input";
    std::string output_section = "output";
    std::string base_config_name = "/config_filename";

    // ros init
    flashros::commons::RosInit(argc, argv, "laser_inference");
    nh_ = new ros::NodeHandle;
    std::string node_name = ros::this_node::getName();

    // Read config file
    std::string config_file_path =
        flashros::commons::GetRosParam(*nh_, node_name + base_config_name);
    flashai::utils::CheckFileExists(config_file_path);
    INIReader cfg_reader = INIReader(config_file_path);
    ROS_WARN_STREAM("config file path " << config_file_path);

    // Init spdlog
    flashai::utils::InitSpdLogger(cfg_reader);
    SPDLOG_INFO("{} successfully init spdlog", node_name);

    // init output related params
    auto output_topic_name =
        cfg_reader.GetString(output_section, "output_topic_name", "");
    ROS_WARN_STREAM("Create publisher for topic " << output_topic_name);
    publisher_ =
        nh_->advertise<flashros_commons::BoxesMsg>(output_topic_name, 1);
    class_id_ = cfg_reader.GetInteger(output_section, "class_id", 0);
    x1_ = cfg_reader.GetInteger(output_section, "x1", 0);
    y1_ = cfg_reader.GetInteger(output_section, "y1", 0);
    x2_ = cfg_reader.GetInteger(output_section, "x2", 0);
    y2_ = cfg_reader.GetInteger(output_section, "y2", 0);

    // params for subscirber
    input_topic_name_ =
        cfg_reader.GetString(input_section, "input_topic_name", "");
  }

  void Run() {
    auto callback_fn = [&](const custom_msg::RANGE::ConstPtr& msg) {
      flashros_commons::BoxesMsg target_msg;
      target_msg.num_objects = 1;
      target_msg.bboxes.push_back(x1_);
      target_msg.bboxes.push_back(y1_);
      target_msg.bboxes.push_back(x2_);
      target_msg.bboxes.push_back(y2_);
      target_msg.classes.push_back(class_id_);
      target_msg.distances.push_back(msg->range);
      target_msg.texts.push_back("");
      target_msg.socres.push_back(1.0);
      target_msg.header.stamp = msg->header.stamp;
      ROS_WARN_STREAM_THROTTLE(1, "publish laser box "
                                       << x1_ << " " << y1_ << " " << x2_ << " "
                                       << y2_ << " " << class_id_ << " "
                                       << msg->range);
      publisher_.publish(target_msg);
    };

    // init subscriber
    ROS_WARN_STREAM("Try to create subscriber with topic "
                    << input_topic_name_ << " and class id " << class_id_);
    auto subscriber =
        nh_->subscribe<custom_msg::RANGE>(input_topic_name_, 1, callback_fn);

    ros::spin();
  }

  void Release() {
    publisher_.shutdown();
    delete nh_;
    SPDLOG_INFO("Ros output release");
  }

 private:
  ros::Publisher publisher_;
  ros::NodeHandle* nh_;
  std::string input_topic_name_;
  int class_id_, x1_, x2_, y1_, y2_;
};

int main(int argc, char** argv) {
  LaserHelper helper;
  helper.Init(argc, argv);
  helper.Run();
  helper.Release();
  return 0;
}